/**
 * @file main.cpp
 * @brief ESP32水上机器人控制系统主程序
 * @description 基于ESP32开发板的双电机水上机器人控制系统，支持通过Dabble App进行蓝牙遥控
 * @author ESP32-AquaBot Team
 * @version 1.0
 * @date 2024
 * 
 * 硬件配置:
 * - 主控: ESP32开发板
 * - 电机驱动: L298N双电机驱动模块
 * - 通信: 蓝牙串口通信
 * - 控制: Dabble App GamePad模块
 * 
 * 功能特性:
 * - 支持前进、后退、左转、右转、停止等基本运动控制
 * - 实时蓝牙通信，响应手机App控制指令
 * - PWM调速控制，可调节电机转速
 * - 完整的状态监控和调试信息输出
 */

#include <BluetoothSerial.h>

/**
 * @brief 蓝牙串口通信对象
 * @description 用于与Dabble App建立蓝牙连接，接收控制指令
 */
BluetoothSerial SerialBT;

// ==================== 硬件引脚定义 ====================
/**
 * @brief 电机引脚定义 - 基于L298N双电机驱动模块接线图
 * @description L298N模块控制两个直流电机，每个电机需要3个控制信号：
 *              - IN1/IN2: 方向控制（正转/反转）
 *              - ENA/ENB: 使能和速度控制（PWM信号）
 */
#define MOTOR_A_IN1 16   ///< 电机A方向控制引脚1 (连接L298N的IN1)
#define MOTOR_A_IN2 17   ///< 电机A方向控制引脚2 (连接L298N的IN2)
#define MOTOR_A_ENA 18   ///< 电机A使能/速度控制引脚 (连接L298N的ENA，PWM控制)
#define MOTOR_B_IN3 19   ///< 电机B方向控制引脚1 (连接L298N的IN3)
#define MOTOR_B_IN4 21   ///< 电机B方向控制引脚2 (连接L298N的IN4)
#define MOTOR_B_ENB 22   ///< 电机B使能/速度控制引脚 (连接L298N的ENB，PWM控制)

// ==================== PWM配置参数 ====================
/**
 * @brief PWM通道定义
 * @description ESP32支持16个独立的PWM通道，这里使用2个通道分别控制两个电机的速度
 */
#define PWM_CHANNEL_ENA 0  ///< 电机A速度控制PWM通道
#define PWM_CHANNEL_ENB 1  ///< 电机B速度控制PWM通道

/**
 * @brief PWM信号参数配置
 * @description 配置PWM信号的频率、分辨率和最大速度值
 */
#define PWM_FREQ 1000      ///< PWM频率 (Hz) - 1kHz适合电机控制
#define PWM_RESOLUTION 8   ///< PWM分辨率 (位) - 8位分辨率，取值范围0-255
#define MAX_SPEED 255      ///< 最大速度值 (对应PWM占空比100%)

// ==================== Dabble协议常量 ====================
/**
 * @brief Dabble通信协议常量定义
 * @description Dabble App使用特定的数据包格式进行通信
 */
#define DABBLE_START_BYTE 0xFF      ///< Dabble数据包起始字节标识
#define DABBLE_GAMEPAD_MODULE 0x01  ///< GamePad模块标识符

// ==================== 游戏手柄状态结构 ====================
/**
 * @brief 游戏手柄按键状态结构体
 * @description 存储GamePad模块所有按键的当前状态
 * 
 * 按键映射:
 * - 方向键: up, down, left, right (控制机器人运动)
 * - 动作键: triangle, cross, circle, square (预留功能扩展)
 * - 系统键: start, select (预留系统控制功能)
 */
struct GamePadState {
  bool up = false;       ///< 方向键：上 (控制前进)
  bool down = false;     ///< 方向键：下 (控制后退)
  bool left = false;     ///< 方向键：左 (控制左转)
  bool right = false;    ///< 方向键：右 (控制右转)
  bool triangle = false; ///< 动作键：三角形 (预留功能)
  bool cross = false;    ///< 动作键：叉号 (预留功能)
  bool circle = false;   ///< 动作键：圆形 (预留功能)
  bool square = false;   ///< 动作键：方形 (预留功能)
  bool start = false;    ///< 系统键：开始 (预留功能)
  bool select = false;   ///< 系统键：选择 (预留功能)
};

/**
 * @brief 当前游戏手柄状态
 * @description 存储当前帧的按键状态，用于实时控制
 */
GamePadState currentState;

/**
 * @brief 上一帧游戏手柄状态
 * @description 存储上一帧的按键状态，用于检测状态变化，避免重复输出
 */
GamePadState lastState;

// ==================== 函数声明 ====================
/**
 * @brief 打印当前游戏手柄按键状态
 * @description 将当前按下的按键信息输出到串口，用于调试和状态监控
 */
void printGamePadState();

/**
 * @brief 根据按键状态控制电机运动
 * @description 解析当前按键状态，调用相应的电机控制函数
 */
void controlMotors();

/**
 * @brief 控制机器人前进
 * @description 两个电机同时正转，实现机器人直线前进
 */
void moveForward();

/**
 * @brief 控制机器人后退
 * @description 两个电机同时反转，实现机器人直线后退
 */
void moveBackward();

/**
 * @brief 控制机器人左转
 * @description 左电机正转，右电机反转，实现机器人原地左转
 */
void turnLeft();

/**
 * @brief 控制机器人右转
 * @description 左电机反转，右电机正转，实现机器人原地右转
 */
void turnRight();

/**
 * @brief 停止所有电机
 * @description 将所有电机控制信号设为低电平，停止机器人运动
 */
void stopMotors();

/**
 * @brief 处理蓝牙接收的Dabble数据
 * @description 从蓝牙串口读取数据，查找并解析Dabble数据包
 */
void processDabbleData();

/**
 * @brief 解析Dabble数据包内容
 * @param data 指向数据包的指针
 * @param length 数据包长度
 * @description 根据Dabble协议解析按键状态，更新currentState结构体
 */
void processDabbleData(uint8_t* data, int length);

/**
 * @brief 系统初始化函数
 * @description Arduino框架的标准初始化函数，在系统启动时执行一次
 *              负责初始化串口、蓝牙、GPIO引脚和PWM通道
 */
void setup() {
  // 初始化串口通信，波特率115200
  Serial.begin(115200);
  
  // 初始化蓝牙串口，设备名称为"ESP32-AquaBot"
  SerialBT.begin("ESP32-AquaBot");
  
  Serial.println("=== 引脚初始化 ===");
  // 设置电机方向控制引脚为输出模式
  pinMode(MOTOR_A_IN1, OUTPUT);
  pinMode(MOTOR_A_IN2, OUTPUT);
  pinMode(MOTOR_B_IN3, OUTPUT);
  pinMode(MOTOR_B_IN4, OUTPUT);
  Serial.println("方向控制引脚初始化完成");
  
  Serial.println("=== PWM通道初始化 ===");
  // 配置PWM通道参数 - 用于电机速度控制
  ledcSetup(PWM_CHANNEL_ENA, PWM_FREQ, PWM_RESOLUTION);
  Serial.printf("PWM通道%d设置完成: 频率=%dHz, 分辨率=%d位 (电机A速度控制)\n", PWM_CHANNEL_ENA, PWM_FREQ, PWM_RESOLUTION);
  
  ledcSetup(PWM_CHANNEL_ENB, PWM_FREQ, PWM_RESOLUTION);
  Serial.printf("PWM通道%d设置完成: 频率=%dHz, 分辨率=%d位 (电机B速度控制)\n", PWM_CHANNEL_ENB, PWM_FREQ, PWM_RESOLUTION);
  
  Serial.println("=== 引脚绑定 ===");
  // 将PWM通道绑定到对应的GPIO引脚
  ledcAttachPin(MOTOR_A_ENA, PWM_CHANNEL_ENA);
  Serial.printf("引脚%d绑定到PWM通道%d (电机A速度控制)\n", MOTOR_A_ENA, PWM_CHANNEL_ENA);
  
  ledcAttachPin(MOTOR_B_ENB, PWM_CHANNEL_ENB);
  Serial.printf("引脚%d绑定到PWM通道%d (电机B速度控制)\n", MOTOR_B_ENB, PWM_CHANNEL_ENB);
  
  // 输出系统启动信息
  Serial.println("ESP32-AquaBot水上机器人控制系统启动");
  Serial.println("等待Dabble App连接...");
  Serial.println("请在Dabble App中选择GamePad模块的Digital模式");
}

/**
 * @brief 主循环函数
 * @description Arduino框架的标准主循环函数，系统运行期间持续执行
 *              负责处理蓝牙数据、检测状态变化、控制电机运动
 */
void loop() {
  // 处理蓝牙接收的控制数据
  processDabbleData();
  
  // 检查按键状态是否发生变化
  bool stateChanged = false;
  if (currentState.up != lastState.up ||
      currentState.down != lastState.down ||
      currentState.left != lastState.left ||
      currentState.right != lastState.right ||
      currentState.triangle != lastState.triangle ||
      currentState.cross != lastState.cross ||
      currentState.circle != lastState.circle ||
      currentState.square != lastState.square ||
      currentState.start != lastState.start ||
      currentState.select != lastState.select) {
    stateChanged = true;
  }
  
  // 如果状态发生变化，输出当前状态信息（避免重复输出）
  if (stateChanged) {
    printGamePadState();
    lastState = currentState;  // 更新上一帧状态
  }
  
  // 根据当前按键状态控制电机运动
  controlMotors();
  
  // 延时50ms，控制循环频率约20Hz
  delay(50);
}

/**
 * @brief 处理蓝牙接收的Dabble数据
 * @description 从蓝牙串口读取可用数据，查找Dabble协议的数据包起始标识，
 *              找到完整的GamePad模块数据包后调用解析函数
 */
void processDabbleData() {
  // 检查蓝牙串口是否有数据可读
  if (SerialBT.available()) {
    uint8_t buffer[32];  // 数据缓冲区，最大32字节
    int bytesRead = 0;   // 已读取的字节数
    
    // 读取所有可用数据到缓冲区
    while (SerialBT.available() && bytesRead < 32) {
      buffer[bytesRead] = SerialBT.read();
      bytesRead++;
    }
    
    // 在缓冲区中查找Dabble数据包
    // 数据包格式: [起始字节0xFF] [模块ID] [数据...]
    for (int i = 0; i < bytesRead - 2; i++) {
      if (buffer[i] == DABBLE_START_BYTE && buffer[i+1] == DABBLE_GAMEPAD_MODULE) {
        // 找到GamePad模块数据包，调用解析函数
        processDabbleData(&buffer[i], bytesRead - i);
        break;
      }
    }
  }
}

/**
 * @brief 解析Dabble GamePad数据包
 * @param data 指向数据包起始位置的指针
 * @param length 数据包长度
 * @description 根据Dabble协议解析GamePad按键状态数据，更新currentState结构体
 * 
 * 数据包格式 (基于实际测试结果):
 * - data[0]: 起始字节 (0xFF)
 * - data[1]: 模块ID (0x01 = GamePad)
 * - data[2-4]: 协议相关字节
 * - data[5]: 动作按键状态 (triangle, circle, cross, square, start, select)
 * - data[6]: 方向键状态 (up, down, left, right)
 */
void processDabbleData(uint8_t* data, int length) {
    if (length >= 5) {
        // 清除之前的按键状态，准备接收新状态
        currentState.up = currentState.down = currentState.left = currentState.right = false;
        currentState.triangle = currentState.cross = currentState.circle = currentState.square = false;
        currentState.start = currentState.select = false;
        
        // 输出原始数据用于调试（十六进制格式）
        Serial.print("原始数据: ");
        for (int i = 0; i < length; i++) {
            Serial.print("0x");
            if (data[i] < 0x10) Serial.print("0");  // 补零对齐
            Serial.print(data[i], HEX);
            Serial.print(" ");
        }
        Serial.println();
        
        // 解析动作按键和特殊按键 (data[5])
        if (length >= 6) {
            uint8_t actionButtons = data[5];
            
            // 按键状态位映射 (基于实际测试数据)
            currentState.start = (actionButtons & 0x01) != 0;      // bit 0: start按键
            currentState.select = (actionButtons & 0x02) != 0;     // bit 1: select按键
            currentState.triangle = (actionButtons & 0x04) != 0;   // bit 2: triangle按键
            currentState.circle = (actionButtons & 0x08) != 0;     // bit 3: circle按键
            currentState.cross = (actionButtons & 0x10) != 0;      // bit 4: cross按键
            currentState.square = (actionButtons & 0x20) != 0;     // bit 5: square按键
            
            // 输出动作按键状态信息
            if (actionButtons != 0x00) {
                Serial.print("动作按键 (data[5]=0x");
                if (actionButtons < 0x10) Serial.print("0");
                Serial.print(actionButtons, HEX);
                Serial.print("): ");
                
                if (currentState.start) Serial.print("START ");
                if (currentState.select) Serial.print("SELECT ");
                if (currentState.triangle) Serial.print("TRIANGLE ");
                if (currentState.circle) Serial.print("CIRCLE ");
                if (currentState.cross) Serial.print("CROSS ");
                if (currentState.square) Serial.print("SQUARE ");
                Serial.println();
            }
        }
        
        // 解析方向键状态 (data[6])
        if (length >= 7) {
            uint8_t directionBits = data[6];
            
            // 方向键状态位映射 (基于实际测试数据)
            currentState.up = (directionBits & 0x01) != 0;      // bit 0: up方向键
            currentState.down = (directionBits & 0x02) != 0;    // bit 1: down方向键
            currentState.left = (directionBits & 0x04) != 0;    // bit 2: left方向键
            currentState.right = (directionBits & 0x08) != 0;   // bit 3: right方向键
            
            // 输出方向键状态信息
            if (directionBits != 0x00) {
                Serial.print("方向键 (data[6]=0x");
                if (directionBits < 0x10) Serial.print("0");
                Serial.print(directionBits, HEX);
                Serial.print("): ");
                
                if (currentState.up) Serial.print("UP ");
                if (currentState.down) Serial.print("DOWN ");
                if (currentState.left) Serial.print("LEFT ");
                if (currentState.right) Serial.print("RIGHT ");
                Serial.println();
            }
        }
        
        // 如果没有任何按键按下，显示空闲状态
        if (data[5] == 0x00 && (length < 7 || data[6] == 0x00)) {
            Serial.println("按键状态: 无按键");
        }
    }
}

/**
 * @brief 打印当前游戏手柄按键状态
 * @description 将当前按下的按键信息以中文形式输出到串口，
 *              用于调试和实时监控用户的操作状态
 */
void printGamePadState() {
  Serial.print("按键状态: ");
  
  // 输出方向键状态
  if (currentState.up) Serial.print("上 ");
  if (currentState.down) Serial.print("下 ");
  if (currentState.left) Serial.print("左 ");
  if (currentState.right) Serial.print("右 ");
  
  // 输出动作按键状态
  if (currentState.triangle) Serial.print("三角 ");
  if (currentState.cross) Serial.print("叉 ");
  if (currentState.circle) Serial.print("圆 ");
  if (currentState.square) Serial.print("方 ");
  
  // 输出系统按键状态
  if (currentState.start) Serial.print("开始 ");
  if (currentState.select) Serial.print("选择 ");
  
  Serial.println();  // 换行结束
}

/**
 * @brief 根据按键状态控制电机运动
 * @description 解析当前的方向键状态，调用相应的电机控制函数实现机器人运动
 *              优先级: 前进 > 后退 > 左转 > 右转 > 停止
 *              注意: 同时按下多个方向键时，按优先级执行第一个有效操作
 */
void controlMotors() {
  // 按优先级检查方向键状态并执行相应动作
  if (currentState.up) {
    moveForward();        // 前进优先级最高
  } else if (currentState.down) {
    moveBackward();       // 后退优先级第二
  } else if (currentState.left) {
    turnLeft();           // 左转优先级第三
  } else if (currentState.right) {
    turnRight();          // 右转优先级第四
  } else {
    stopMotors();         // 无按键时停止电机
  }
}

/**
 * @brief 控制机器人前进
 * @description 设置两个电机同时正转，实现机器人直线前进运动
 * 
 * 电机控制逻辑:
 * - 电机A: IN1=HIGH, IN2=LOW (正转), ENA=MAX_SPEED (全速)
 * - 电机B: IN3=HIGH, IN4=LOW (正转), ENB=MAX_SPEED (全速)
 */
void moveForward() {
  Serial.println("=== 前进控制 ===");
  Serial.printf("电机A: IN1=%d(HIGH), IN2=%d(LOW), ENA=%d(PWM=%d)\n", 
                MOTOR_A_IN1, MOTOR_A_IN2, MOTOR_A_ENA, MAX_SPEED);
  Serial.printf("电机B: IN3=%d(HIGH), IN4=%d(LOW), ENB=%d(PWM=%d)\n", 
                MOTOR_B_IN3, MOTOR_B_IN4, MOTOR_B_ENB, MAX_SPEED);
  
  // 设置电机A正转: IN1=HIGH, IN2=LOW, ENA=PWM
  digitalWrite(MOTOR_A_IN1, HIGH);
  digitalWrite(MOTOR_A_IN2, LOW);
  ledcWrite(PWM_CHANNEL_ENA, MAX_SPEED);
  
  // 设置电机B正转: IN3=HIGH, IN4=LOW, ENB=PWM
  digitalWrite(MOTOR_B_IN3, HIGH);
  digitalWrite(MOTOR_B_IN4, LOW);
  ledcWrite(PWM_CHANNEL_ENB, MAX_SPEED);
  
  Serial.println("电机控制信号已发送");
}

/**
 * @brief 控制机器人后退
 * @description 设置两个电机同时反转，实现机器人直线后退运动
 * 
 * 电机控制逻辑:
 * - 电机A: IN1=LOW, IN2=HIGH (反转), ENA=MAX_SPEED (全速)
 * - 电机B: IN3=LOW, IN4=HIGH (反转), ENB=MAX_SPEED (全速)
 */
void moveBackward() {
  Serial.println("=== 后退控制 ===");
  
  // 设置电机A反转: IN1=LOW, IN2=HIGH, ENA=PWM
  digitalWrite(MOTOR_A_IN1, LOW);
  digitalWrite(MOTOR_A_IN2, HIGH);
  ledcWrite(PWM_CHANNEL_ENA, MAX_SPEED);
  
  // 设置电机B反转: IN3=LOW, IN4=HIGH, ENB=PWM
  digitalWrite(MOTOR_B_IN3, LOW);
  digitalWrite(MOTOR_B_IN4, HIGH);
  ledcWrite(PWM_CHANNEL_ENB, MAX_SPEED);
}

/**
 * @brief 控制机器人左转
 * @description 设置左电机正转、右电机反转，实现机器人原地左转
 * 
 * 电机控制逻辑:
 * - 电机A(左): IN1=HIGH, IN2=LOW (正转), ENA=MAX_SPEED
 * - 电机B(右): IN3=LOW, IN4=HIGH (反转), ENB=MAX_SPEED
 * 
 * 运动原理: 左右电机转向相反，产生转矩使机器人原地旋转
 */
void turnLeft() {
  Serial.println("=== 左转控制 ===");
  
  // 设置电机A(左)正转: IN1=HIGH, IN2=LOW, ENA=PWM
  digitalWrite(MOTOR_A_IN1, HIGH);
  digitalWrite(MOTOR_A_IN2, LOW);
  ledcWrite(PWM_CHANNEL_ENA, MAX_SPEED);
  
  // 设置电机B(右)反转: IN3=LOW, IN4=HIGH, ENB=PWM
  digitalWrite(MOTOR_B_IN3, LOW);
  digitalWrite(MOTOR_B_IN4, HIGH);
  ledcWrite(PWM_CHANNEL_ENB, MAX_SPEED);
}

/**
 * @brief 控制机器人右转
 * @description 设置左电机反转、右电机正转，实现机器人原地右转
 * 
 * 电机控制逻辑:
 * - 电机A(左): IN1=LOW, IN2=HIGH (反转), ENA=MAX_SPEED
 * - 电机B(右): IN3=HIGH, IN4=LOW (正转), ENB=MAX_SPEED
 * 
 * 运动原理: 左右电机转向相反，产生转矩使机器人原地旋转
 */
void turnRight() {
  Serial.println("=== 右转控制 ===");
  
  // 设置电机A(左)反转: IN1=LOW, IN2=HIGH, ENA=PWM
  digitalWrite(MOTOR_A_IN1, LOW);
  digitalWrite(MOTOR_A_IN2, HIGH);
  ledcWrite(PWM_CHANNEL_ENA, MAX_SPEED);
  
  // 设置电机B(右)正转: IN3=HIGH, IN4=LOW, ENB=PWM
  digitalWrite(MOTOR_B_IN3, HIGH);
  digitalWrite(MOTOR_B_IN4, LOW);
  ledcWrite(PWM_CHANNEL_ENB, MAX_SPEED);
}

/**
 * @brief 停止所有电机
 * @description 将所有电机控制信号设为低电平，完全停止机器人运动
 * 
 * 电机控制逻辑:
 * - 电机A: IN1=LOW, IN2=LOW (停止), ENA=0 (无PWM信号)
 * - 电机B: IN3=LOW, IN4=LOW (停止), ENB=0 (无PWM信号)
 * 
 * 安全特性: 通过设置方向引脚为LOW和PWM为0，确保电机完全停止
 */
void stopMotors() {
  Serial.println("=== 停止控制 ===");
  
  // 停止电机A: IN1=LOW, IN2=LOW, ENA=0
  digitalWrite(MOTOR_A_IN1, LOW);
  digitalWrite(MOTOR_A_IN2, LOW);
  ledcWrite(PWM_CHANNEL_ENA, 0);
  
  // 停止电机B: IN3=LOW, IN4=LOW, ENB=0
  digitalWrite(MOTOR_B_IN3, LOW);
  digitalWrite(MOTOR_B_IN4, LOW);
  ledcWrite(PWM_CHANNEL_ENB, 0);
}